#include "pqueue-widx.h"
void PQueueWidx::push_back(int val) {
    pqv.push_back(val);
    if(val>= qidxv.size()) {
        qidxv.resize(val*2+1);
    }
    qidxv[val] = pqv.size()-1;
}


int PQueueWidx::dequeue(void) {
    int tmp = pqv[0];
    pop();
    return tmp;
}

void PQueueWidx::pop(void) {
    pqv.pop_back();
    pqv[0] = pqv[pqv.size()];
    qidxv[pqv[0]] = 0;
    down_fix_heap(pqv[0]);
}

void PQueueWidx::push(int val) {
    push_back(val);
    up_fix_heap(val);   
}

void PQueueWidx::up_fix_heap(int val) {
    int idx = qidxv[val];
    while(idx>0) {
        int par = (idx-1)/2;
        if(cmp(pqv[par], val)) break;
        pqv[idx] = pqv[par];
        qidxv[pqv[idx]] = idx;
        idx = par;
    }
    pqv[idx] = val;
    qidxv[val] = idx;
}

void PQueueWidx::down_fix_heap(int val) {
    int idx = qidxv[val];
    int lc = idx*2+1;
    int rc;
    while(lc < pqv.size()) {
        int midx = idx; 
        int mval = val;
        if(cmp(pqv[lc], mval)) {
            mval = pqv[lc];
            midx = lc;
        }
        rc = lc+1;
        if(rc<pqv.size() && cmp(pqv[rc], mval)) {
            midx = rc;
            mval = pqv[rc];
        }
        if(idx == midx) break;
        pqv[idx] = mval;
        qidxv[mval] = idx;
        idx = midx;
        lc = idx*2+1;
        
    }
    pqv[idx] = val;
    qidxv[val] = idx;
}

void PQueueWidx::make_heap(void) {
    int idx = pqv.size()/2-1;
    while(idx>=0) {
        down_fix_heap(qidxv[idx--]);
    }
}


